#pragma config(Sensor, in1,    Mode,                sensorPotentiometer)
#pragma config(Sensor, in2,    LeftLine,            sensorLineFollower)
#pragma config(Sensor, in3,    MidLine,             sensorLineFollower)
#pragma config(Sensor, in4,    RightLine,           sensorLineFollower)
#pragma config(Sensor, in5,    EncoderR,        sensorQuadEncoder, int3)
#pragma config(Sensor, in6,    EncoderL,         sensorQuadEncoder, int4)
#pragma config(Motor,  port2,           DriveR,    tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveL,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* DRIVE FUNCTIONS
 * drive_forward_1msec

 * drive_straight
 * drive_straight_to_touch
 */

 /* drive_forward_msec
  * @purpose: autonomously drive robot forward for provided duration
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param duration: duration in milliseconds
 */

void drive_forward_msec(int speed, int duration) {
  motor[DriveR] = speed;
  motor[DriveL] = speed;
  wait1Msec(duration);
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}
/* drive_straight
  * @purpose: autonomously drive robot forward for provided distance in inches
  * precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
  * @param speed: speed of motors, -127 to 127; negative values go reverse
  * @param distance: distance in inches
  * TODO: consider adding an acceleration loop
 *///there are 360 ticks in an encoder revolution
void drive_straight(int speed, int distance) {
  SensorValue[EncoderR] = 0;
  SensorValue[EncoderL] = 0;
  while( (SensorValue[EncoderR]  * 4 / 360 * 3.14) < distance &&
         (SensorValue[EncoderL] * 4 / 360 * 3.14) < distance) {
    if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
      motor[DriveR] = speed;
      motor[DriveL]  = speed - 1;
    }
    else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = speed - 1;
      motor[DriveL]  = speed;
    }
    else {
      motor[DriveR] = speed;
      motor[DriveL]  = speed;
    }
  }
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}

/* drive_straight_to_touch
  * @purpose: autonomously drive robot forward until touch sensors are triggered
  * precondition: 2 touch sensors, left and right
  * @param speed: speed of motors, -127 to 127; negative values go reverse
 */
/*void drive_straight_to_touch(int speed) {
  SensorValue[EncoderR] = 0;
  SensorValue[EncoderL] = 0;
  while(SensorValue[LeftTouch]  == 0 || SensorValue[RightTouch] == 0) {
    if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
      motor[DriveR] = speed;
      motor[DriveL]  = speed - 1;
    }
    else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = speed - 1;
      motor[DriveL]  = speed;
    }
    else {
      motor[DriveR] = speed;
      motor[DriveL]  = speed;
    }
  }
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}*/

/* turn
  * @purpose: autonomously execute a point turn; unique to robot dependent upon wheel radius
  * @param speed: speed of motors, 0 to 127; no negative values
  * @param degrees: number of degrees to turn
  * TODO: complete this function
 */
void turn(int speed, int degrees) {
  //exact implementation will be unique to robot
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}


/* MODE SELECTION FUNCTION
 * mode_select
 * @purpose use a potentiometer to set mode
 * high potentiometer values return 1, low potentiometer values return 0
 */

int mode_select() {
  if(SensorValue[Mode] >= 2000)
    return 1;
  else
    return 0;

}


/* LINE FOLLOWER FUNCTIONS
 * follow_line_msec
 * follow_line_distance
 */

 /*follow_line_msec
 * @purpose follow a line for provided duration using 3 light sensors
 * @param speed: speed of motors, 0 to 127
 * @param duration: duration of line following in milliseconds
 */
/*void follow_line_msec(int speed, int duration) {
  ClearTimer(T1);
  int threshold = 250;  //guessed threshold; low numbers are lighter
  while(time1[T1] < duration) {
    if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
     motor[DriveL]  = speed;
     motor[DriveR] = speed;
    }
    else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
      motor[DriveL]  = speed - 1;
      motor[DriveR] = speed;
    }
    else if(SensorValue[RightLine] < threshold) { //right sensor is on line
      motor[DriveL]  = speed;
      motor[DriveR] = speed - 1;
    }
  }
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}*/


 /*follow_line_distance
 * @purpose follow a line for provided duration using 3 light sensors
 * precondition: 4-inch wheels
 * @param speed: speed of motors, 0 to 127
 * @param distance: distance to travel in inches
 */
/*void follow_line_distance(int speed, int distance) {
  SensorValue[EncoderR] = 0;
  SensorValue[EncoderL] = 0;
  int threshold = 250;  //guessed threshold; low numbers are lighter
  while(((float)SensorValue[EncoderL]+(float)SensorValue[EncoderR])/2 * 4 / 360 * 3.14
                                                                      < distance){
    if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
     motor[DriveL]  = speed;
     motor[DriveR] = speed;
    }
    else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
      motor[DriveL]  = speed - 1;
      motor[DriveR] = speed;
    }
    else if(SensorValue[RightLine] < threshold) { //right sensor is on line
      motor[DriveL]  = speed;
      motor[DriveR] = speed - 1;
    }
  }
  motor[DriveR] = 0;
  motor[DriveL] = 0;
}*/
